#include "camera.h"
#include "scene.h"
#include "includes.h"
#include <iostream>

Camera::Camera()
{
	view_matrix.setIdentity();
	setOrthographic(-100,100,100,-100,-100,100);
}

void Camera::set()
{
	updateViewMatrix();
	updateProjectionMatrix();

	glMatrixMode( GL_MODELVIEW );
	glLoadMatrixf( view_matrix.m );

	glMatrixMode( GL_PROJECTION );
	glLoadMatrixf( projection_matrix.m );

	glMatrixMode( GL_MODELVIEW );
}

void Camera::setOffset(Vector3 _offset){
	offset = -_offset;
}

Vector3 Camera::getLocalVector(const Vector3& v)
{
	Matrix44 iV = view_matrix;
	if (iV.inverse() == false)
		std::cout << "Matrix Inverse error" << std::endl;
	Vector3 result = iV.rotateVector(v);
	return result;
}

void Camera::move(Vector3 delta)
{
	Vector3 localDelta = getLocalVector(delta);
	eye = eye - localDelta;
	center = center - localDelta;
	updateViewMatrix();
}

void Camera::rotate(float angle, const Vector3& axis)
{
	Matrix44 R;
	R.setRotation(angle,axis);
	Vector3 new_front = R * (center - eye);
	center = eye + new_front;
	updateViewMatrix();
}

void Camera::setOrthographic(float left, float right, float top, float bottom, float near_plane, float far_plane)
{
	type = ORTHOGRAPHIC;

	this->left = left;
	this->right = right;
	this->top = top;
	this->bottom = bottom;
	this->near_plane = near_plane;
	this->far_plane = far_plane;

	updateProjectionMatrix();
}

void Camera::setPerspective(float fov, float aspect, float near_plane, float far_plane)
{
	type = PERSPECTIVE;

	this->fov = fov;
	this->aspect = aspect;
	this->near_plane = near_plane;
	this->far_plane = far_plane;

	//update projection
	updateProjectionMatrix();
}

void Camera::lookAt(const Vector3& eye, const Vector3& center, const Vector3& up)
{
	this->eye = eye;
	this->center = center;
	this->up = up;

	updateViewMatrix();
}

void Camera::updateViewMatrix()
{
	if (type != PERSPECTIVE)
		return;

	//We activate the matrix we want to work: modelview
	glMatrixMode(GL_MODELVIEW);

	//We set it as identity
	glLoadIdentity();
	
	//We find the look at matrix
	//eye = eye * offset;
	gluLookAt( eye.x, eye.y, eye.z, center.x, center.y, center.z, up.x, up.y, up.z);

	//We get the matrix and store it in our app
	glGetFloatv(GL_MODELVIEW_MATRIX, view_matrix.m );
}

// ******************************************

//Create a projection matrix
void Camera::updateProjectionMatrix()
{
	//We activate the matrix we want to work: projection
	glMatrixMode(GL_PROJECTION);

	//We set it as identity
	glLoadIdentity();

	if (type == PERSPECTIVE)
		gluPerspective(fov, aspect, near_plane, far_plane);
	else
		glOrtho(left,right,bottom,top,near_plane,far_plane);

	//upload to hardware
	glGetFloatv(GL_PROJECTION_MATRIX, projection_matrix.m );

	glMatrixMode(GL_MODELVIEW);
}

bool Camera::isAhead(Vector3 _pos)
{
        Vector3 front = center - eye;
        Vector3 planeNormal = front;
        float d = -planeNormal.dot(eye);
        float planeFunc = planeNormal.dot(_pos) + d;
        if(planeFunc >= 0)
                return true;
        return false;
}

void Camera::folowAt(Instance *_follow){
	
	this->follow = _follow;
}
void Camera::update(){

	if(this->follow != NULL){
		float a,b;
		float offset;
		std::string name_mesh;
		Scene * scene;
		Vector3 tmp, new_pos, new_eye, new_up;
		Matrix44 * object;

		scene = Scene::getScene();
		object = this->follow->model;
		name_mesh = this->follow->id_mesh;
		tmp = scene->loaded_meshes[name_mesh]->lejano - scene->loaded_meshes[name_mesh]->media;
		offset = sqrt(tmp.x*tmp.x+tmp.y*tmp.y+tmp.z*tmp.z)+4.0f;
		this->setOffset((Vector3(0,14,50).normalize() * offset * 2));
		new_pos = *object * Vector3(0,0,0);
		new_eye = *object * (Vector3(0,14,50).normalize() * offset * 2);
		new_up  = object->rotateVector(Vector3(0,1,0));
		
		a = 1 - offset/80;
		b = offset / 80;
		this->eye = this->eye * 0.89 + new_eye * 0.11;
		this->up = new_up;
		this->center = new_pos;
		this->updateViewMatrix();
		this->updateProjectionMatrix();
		
	}else{
		//Log; Cam follower invalid
	}
}